#include "scan_matching/pose_observe.h"

static double theta_normalize(const double &theta) {
    return std::atan2(std::sin(theta), std::cos(theta));
}
// predict - obs
void PoseObserve::ComputeResidualAndJacobianPoseGraph(const State& state, const State& pose_state, Eigen::Matrix<double, 3, 1> &residualXi, Eigen::Matrix<double, 3, 3> &jacobianXi) {
	Eigen::Vector2d res_p = state.p - pose_state.p;
	double res_theta = theta_normalize(SO2(state.R).log() - SO2(pose_state.R).log());

	residualXi.block<2,1>(0,0) = res_p;
	residualXi(2,0) = res_theta;

	// cout << "residualXi ================== : " << residualXi << endl;

	jacobianXi = Eigen::Matrix<double, 3, 3>::Identity();
	
    // cout << "jacobianXi == : \n" << jacobianXi << endl;
}

